#include "park_task.h"
#include "kernel/agv/agv_entity.h"
#include <QString>
#include <algorithm>
#include <limits>

namespace application {
namespace task {

ParkTask::ParkTask(
    std::shared_ptr<vehicle::VehicleService> vehicleService,
    std::shared_ptr<MapApplicationService> mapService,
    std::shared_ptr<dispatcher::DispatcherService> dispatcherService,
    std::shared_ptr<kernel::ILogger> logger,
    const std::string& parkingStation,
    int floor)
    : SubTaskBase(logger)
    , m_vehicleService(vehicleService)
    , m_mapService(mapService)
    , m_dispatcherService(dispatcherService)
    , m_parkingStation(parkingStation)
    , m_floor(floor)
{
}

kernel::task::TaskStage ParkTask::init(TaskContext& ctx)
{
    // 参考 task_park.cpp:11-86

    if (!ctx.agv) {
        return finishWithError(ctx, "No AGV assigned to park task");
    }

    // 检查是否已有泊车点（参考 task_park.cpp:13-15）
    if (!m_allocatedParkPoint) {
        if (!requestParkingPoint(ctx)) {
            return finishWithError(ctx, "Failed to request parking point");
        }

        logInfo("Parking point allocated: " +
                m_allocatedParkPoint->getPointId().toStdString());

        ctx.requestStatus(kernel::task::TaskChainStatus::Recalculation);
        return kernel::task::TaskStage::Init;
    }

    m_startTime = std::chrono::system_clock::now();
    return kernel::task::TaskStage::Running;
}

kernel::task::TaskStage ParkTask::run(TaskContext& ctx)
{
    // 参考 task_park.cpp:89-100

    if (!ctx.agv) {
        return finishWithError(ctx, "AGV lost during parking");
    }

    if (!m_allocatedParkPoint) {
        return finishWithError(ctx, "Parking point not allocated");
    }

    // 检查是否到达泊车点（参考 task_park.cpp:91-96）
    if (hasArrivedAtParkingPoint(ctx)) {
        logInfo("AGV " + std::to_string(ctx.agv->getNumber()) +
                " arrived at parking point: " +
                m_allocatedParkPoint->getPointId().toStdString());

        return kernel::task::TaskStage::Cleanup;
    }

    // 继续等待到达
    return kernel::task::TaskStage::Running;
}

kernel::task::TaskStage ParkTask::cleanup(TaskContext& ctx)
{
    // 参考 task_park.cpp:102-107

    logDebug("Park task cleanup");

    // 泊车任务完成，区域会在任务链结束时统一释放
    // 参考三方代码：cleanup 只是简单地标记进入下一阶段
    return kernel::task::TaskStage::Finished;
}

// ==================== 私有辅助方法 ====================

bool ParkTask::requestParkingPoint(TaskContext& ctx)
{
    // 参考 task_park.cpp:17-33

    if (!m_mapService) {
        logError("MapService not available");
        return false;
    }

    int targetFloor = resolveTargetFloor(ctx);
    if (targetFloor == -1) {
        logError("Unable to resolve floor for parking task");
        return false;
    }

    // 获取泊车管理器
    auto parkManager = m_mapService->getParkManager(targetFloor);
    if (!parkManager) {
        logError("ParkManager not found for floor: " + std::to_string(targetFloor));
        return false;
    }

    if (!m_parkingStation.empty()) {
        auto manager = m_mapService->getParkManager(targetFloor);
        if (!manager) {
            logError("ParkManager not found for floor: " + std::to_string(targetFloor));
            return false;
        }

        m_allocatedParkPoint = manager->getParkPoint(
            QString::fromStdString(m_parkingStation)
        );

        if (!m_allocatedParkPoint) {
            logError("Specified parking point not found: " + m_parkingStation);
            return false;
        }

        if (m_allocatedParkPoint->isOccupied()) {
            logError("Specified parking point is occupied: " + m_parkingStation);
            return false;
        }
    } else {
        if (!m_dispatcherService) {
            logError("DispatcherService unavailable for parking allocation");
            return false;
        }
        m_allocatedParkPoint = m_dispatcherService->allocateParkingPoint(ctx.agv, targetFloor);
        if (!m_allocatedParkPoint) {
            logError("No available parking point found for AGV");
            return false;
        }
    }

    if (ctx.agv) {
        if (!m_allocatedParkPoint->preOccupy(ctx.agv->getNumber())) {
            logError("Failed to pre-occupy parking point: " +
                    m_allocatedParkPoint->getPointId().toStdString());
            m_allocatedParkPoint = nullptr;
            return false;
        }
    }

    if (auto area = m_allocatedParkPoint->getArea()) {
        ctx.addOccupiedArea(area);
    }

    ctx.setParkPoint(m_allocatedParkPoint);

    return true;
}

bool ParkTask::hasArrivedAtParkingPoint(const TaskContext& ctx) const
{
    // 参考 task_park.cpp:91-96

    if (!ctx.agv || !m_allocatedParkPoint) {
        return false;
    }

    // 检查当前站点是否是泊车点
    std::string currentStation = ctx.agv->getCurrentStation();
    std::string parkStation = m_allocatedParkPoint->getStationName().toStdString();
    if (parkStation.empty()) {
        return false;
    }

    bool atStation = (currentStation == parkStation);

    // 检查导航状态是否完成（task_status == 4 || 5）
    auto navStatus = ctx.agv->getNavStatus();
    bool navComplete = (navStatus == kernel::agv::NavStatus::Completed ||
                       navStatus == kernel::agv::NavStatus::Canceled);

    return atStation && navComplete;
}

int ParkTask::resolveTargetFloor(const TaskContext& ctx) const
{
    if (!m_mapService) {
        return -1;
    }

    if (ctx.agv) {
        const auto& mapName = ctx.agv->getCurrentMap();
        if (!mapName.empty()) {
            return m_mapService->getFloorForMapName(QString::fromStdString(mapName));
        }
    }

    return m_mapService->getCurrentFloor();
}

} // namespace task
} // namespace application

